typedef GMapPoint GDAL_GCP;
#define rX  x
#define rY  y
#define gX  g.x
#define gY  g.y
#define ABS abs

/**
 * Generate Geotransform from GCPs.
 *
 * Given a set of GCPs perform first order fit as a geotransform.
 *
 * Due to imprecision in the calculations the fit algorithm will often
 * return non-zero rotational coefficients even if given perfectly non-rotated
 * inputs.  A special case has been implemented for corner corner coordinates
 * given in TL, TR, BR, BL order.  So when using this to get a geotransform
 * from 4 corner coordinates, pass them in this order.
 *
 * @param nGCPCount the number of GCPs being passed in.
 * @param pasGCPs the list of GCP structures.
 * @param padfGeoTransform the six double array in which the affine
 * geotransformation will be returned.
 * @param bApproxOK If FALSE the function will fail if the geotransform is not
 * essentially an exact fit (within 0.25 pixel) for all GCPs.
 *
 * @return TRUE on success or FALSE if there aren't enough points to prepare a
 * geotransform, the pointers are ill-determined or if bApproxOK is FALSE
 * and the fit is poor.
 */

int GDALGCPsToGeoTransform_LatLon( int nGCPCount, const GDAL_GCP *pasGCPs,
                            double *padfGeoTransform, int bApproxOK )

{
    int    i;

/* -------------------------------------------------------------------- */
/*      Recognise a few special cases.                                  */
/* -------------------------------------------------------------------- */
    if( nGCPCount < 2 )
        return FALSE;

    if( nGCPCount == 2 )
    {
        if( pasGCPs[1].x == pasGCPs[0].x ||
            pasGCPs[1].y == pasGCPs[0].y )
            return FALSE;

        padfGeoTransform[1] = ((double)pasGCPs[1].g.x - pasGCPs[0].g.x) / ((double)pasGCPs[1].x - pasGCPs[0].x);
        padfGeoTransform[2] = 0.0;

        padfGeoTransform[4] = 0.0;
        padfGeoTransform[5] = ((double)pasGCPs[1].g.y - pasGCPs[0].g.y) / ((double)pasGCPs[1].y - pasGCPs[0].y);

        padfGeoTransform[0] = pasGCPs[0].g.x - pasGCPs[0].x * padfGeoTransform[1] - pasGCPs[0].y * padfGeoTransform[2];

        padfGeoTransform[3] = pasGCPs[0].g.y - pasGCPs[0].x * padfGeoTransform[4] - pasGCPs[0].y * padfGeoTransform[5];

        return TRUE;
    }

/* -------------------------------------------------------------------- */
/*      Special case of 4 corner coordinates of a non-rotated           */
/*      image.  The points must be in TL-TR-BR-BL order for now.        */
/*      This case helps avoid some imprecision in the general           */
/*      calcuations.                                                    */
/* -------------------------------------------------------------------- */
    if( nGCPCount == 4
        && pasGCPs[0].y == pasGCPs[1].y
        && pasGCPs[2].y == pasGCPs[3].y
        && pasGCPs[0].x == pasGCPs[3].x
        && pasGCPs[1].x == pasGCPs[2].x
        && pasGCPs[0].y != pasGCPs[2].y
        && pasGCPs[0].x != pasGCPs[1].x
        && pasGCPs[0].g.y == pasGCPs[1].g.y
        && pasGCPs[2].g.y == pasGCPs[3].g.y
        && pasGCPs[0].g.x == pasGCPs[3].g.x
        && pasGCPs[1].g.x == pasGCPs[2].g.x
        && pasGCPs[0].g.y != pasGCPs[2].g.y
        && pasGCPs[0].g.x != pasGCPs[1].g.x )
    {
        padfGeoTransform[1] = (pasGCPs[1].g.x - pasGCPs[0].g.x) / (pasGCPs[1].x - pasGCPs[0].x);
        padfGeoTransform[2] = 0.0;
        padfGeoTransform[4] = 0.0;
        padfGeoTransform[5] = (pasGCPs[2].g.y - pasGCPs[1].g.y) / (pasGCPs[2].y - pasGCPs[1].y);

        padfGeoTransform[0] = pasGCPs[0].g.x - pasGCPs[0].x * padfGeoTransform[1];
        padfGeoTransform[3] = pasGCPs[0].g.y - pasGCPs[0].y * padfGeoTransform[5];
        return TRUE;
    }

/* -------------------------------------------------------------------- */
/* In the general case, do a least squares error approximation by       */
/* solving the equation Sum[(A - B*x + C*y - Lon)^2] = minimum      */
/* -------------------------------------------------------------------- */

    double sum_x = 0.0, sum_y = 0.0, sum_xy = 0.0, sum_xx = 0.0, sum_yy = 0.0;
    double sum_Lon = 0.0, sum_Lonx = 0.0, sum_Lony = 0.0;
    double sum_Lat = 0.0, sum_Latx = 0.0, sum_Laty = 0.0;
    double divisor;

    for (i = 0; i < nGCPCount; ++i) {
        sum_x += pasGCPs[i].x;
        sum_y += pasGCPs[i].y;
        sum_xy += pasGCPs[i].x * pasGCPs[i].y;
        sum_xx += pasGCPs[i].x * pasGCPs[i].x;
        sum_yy += pasGCPs[i].y * pasGCPs[i].y;
        sum_Lon += pasGCPs[i].g.x;
        sum_Lonx += pasGCPs[i].g.x * pasGCPs[i].x;
        sum_Lony += pasGCPs[i].g.x * pasGCPs[i].y;
        sum_Lat += pasGCPs[i].g.y;
        sum_Latx += pasGCPs[i].g.y * pasGCPs[i].x;
        sum_Laty += pasGCPs[i].g.y * pasGCPs[i].y;
    }

    divisor = nGCPCount * (sum_xx * sum_yy - sum_xy * sum_xy)
        + 2 * sum_x * sum_y * sum_xy - sum_y * sum_y * sum_xx
        - sum_x * sum_x * sum_yy;

/* -------------------------------------------------------------------- */
/*      If the divisor is zero, there is no valid solution.             */
/* -------------------------------------------------------------------- */
    if (divisor == 0.0)
        return FALSE;

/* -------------------------------------------------------------------- */
/*      Compute top/left origin.                                        */
/* -------------------------------------------------------------------- */

    padfGeoTransform[0] = (sum_Lon * (sum_xx * sum_yy - sum_xy * sum_xy)
                           + sum_Lonx * (sum_y * sum_xy - sum_x *  sum_yy)
                           + sum_Lony * (sum_x * sum_xy - sum_y * sum_xx))
        / divisor;

    padfGeoTransform[3] = (sum_Lat * (sum_xx * sum_yy - sum_xy * sum_xy)
                           + sum_Latx * (sum_y * sum_xy - sum_x *  sum_yy)
                           + sum_Laty * (sum_x * sum_xy - sum_y * sum_xx))
        / divisor;

/* -------------------------------------------------------------------- */
/*      Compute X related coefficients.                                 */
/* -------------------------------------------------------------------- */
    padfGeoTransform[1] = (sum_Lon * (sum_y * sum_xy - sum_x * sum_yy)
                           + sum_Lonx * (nGCPCount * sum_yy - sum_y * sum_y)
                           + sum_Lony * (sum_x * sum_y - sum_xy * nGCPCount))
        / divisor;

    padfGeoTransform[2] = (sum_Lon * (sum_x * sum_xy - sum_y * sum_xx)
                           + sum_Lonx * (sum_x * sum_y - nGCPCount * sum_xy)
                           + sum_Lony * (nGCPCount * sum_xx - sum_x * sum_x))
        / divisor;

/* -------------------------------------------------------------------- */
/*      Compute Y related coefficients.                                 */
/* -------------------------------------------------------------------- */
    padfGeoTransform[4] = (sum_Lat * (sum_y * sum_xy - sum_x * sum_yy)
                           + sum_Latx * (nGCPCount * sum_yy - sum_y * sum_y)
                           + sum_Laty * (sum_x * sum_y - sum_xy * nGCPCount))
        / divisor;

    padfGeoTransform[5] = (sum_Lat * (sum_x * sum_xy - sum_y * sum_xx)
                           + sum_Latx * (sum_x * sum_y - nGCPCount * sum_xy)
                           + sum_Laty * (nGCPCount * sum_xx - sum_x * sum_x))
        / divisor;

/* -------------------------------------------------------------------- */
/*      Now check if any of the input points fit this poorly.           */
/* -------------------------------------------------------------------- */
    if( !bApproxOK )
    {
        double dfPixelSize = ABS(padfGeoTransform[1])
            + ABS(padfGeoTransform[2])
            + ABS(padfGeoTransform[4])
            + ABS(padfGeoTransform[5]);

        for( i = 0; i < nGCPCount; i++ )
        {
            double      dfErrorX, dfErrorY;

            dfErrorX =
                (pasGCPs[i].x * padfGeoTransform[1]
                 + pasGCPs[i].y * padfGeoTransform[2]
                 + padfGeoTransform[0])
                - pasGCPs[i].g.x;
            dfErrorY =
                (pasGCPs[i].x * padfGeoTransform[4]
                 + pasGCPs[i].y * padfGeoTransform[5]
                 + padfGeoTransform[3])
                - pasGCPs[i].g.y;

            if( ABS(dfErrorX) > 0.25 * dfPixelSize
                || ABS(dfErrorY) > 0.25 * dfPixelSize )
                return FALSE;
        }
    }

    return TRUE;
}

int GDALGCPsToGeoTransform_NE( int nGCPCount, const GDAL_GCP *pasGCPs,
                            double *padfGeoTransform, int bApproxOK )

{
    int    i;

/* -------------------------------------------------------------------- */
/*      Recognise a few special cases.                                  */
/* -------------------------------------------------------------------- */
    if( nGCPCount < 2 )
        return FALSE;

    if( nGCPCount == 2 )
    {
        if( pasGCPs[1].x == pasGCPs[0].x ||
            pasGCPs[1].y == pasGCPs[0].y )
            return FALSE;

        padfGeoTransform[1] = ((double)pasGCPs[1].ne.e - pasGCPs[0].ne.e) / ((double)pasGCPs[1].x - pasGCPs[0].x);
        padfGeoTransform[2] = 0.0;

        padfGeoTransform[4] = 0.0;
        padfGeoTransform[5] = ((double)pasGCPs[1].ne.n - pasGCPs[0].ne.n) / ((double)pasGCPs[1].y - pasGCPs[0].y);

        padfGeoTransform[0] = pasGCPs[0].ne.e - pasGCPs[0].x * padfGeoTransform[1] - pasGCPs[0].y * padfGeoTransform[2];

        padfGeoTransform[3] = pasGCPs[0].ne.n - pasGCPs[0].x * padfGeoTransform[4] - pasGCPs[0].y * padfGeoTransform[5];

        return TRUE;
    }

/* -------------------------------------------------------------------- */
/*      Special case of 4 corner coordinates of a non-rotated           */
/*      image.  The points must be in TL-TR-BR-BL order for now.        */
/*      This case helps avoid some imprecision in the general           */
/*      calcuations.                                                    */
/* -------------------------------------------------------------------- */
    if( nGCPCount == 4
        && pasGCPs[0].y == pasGCPs[1].y
        && pasGCPs[2].y == pasGCPs[3].y
        && pasGCPs[0].x == pasGCPs[3].x
        && pasGCPs[1].x == pasGCPs[2].x
        && pasGCPs[0].y != pasGCPs[2].y
        && pasGCPs[0].x != pasGCPs[1].x
        && pasGCPs[0].ne.n == pasGCPs[1].ne.n
        && pasGCPs[2].ne.n == pasGCPs[3].ne.n
        && pasGCPs[0].ne.e == pasGCPs[3].ne.e
        && pasGCPs[1].ne.e == pasGCPs[2].ne.e
        && pasGCPs[0].ne.n != pasGCPs[2].ne.n
        && pasGCPs[0].ne.e != pasGCPs[1].ne.e )
    {
        padfGeoTransform[1] = (pasGCPs[1].ne.e - pasGCPs[0].ne.e) / (pasGCPs[1].x - pasGCPs[0].x);
        padfGeoTransform[2] = 0.0;
        padfGeoTransform[4] = 0.0;
        padfGeoTransform[5] = (pasGCPs[2].ne.n - pasGCPs[1].ne.n) / (pasGCPs[2].y - pasGCPs[1].y);

        padfGeoTransform[0] = pasGCPs[0].ne.e - pasGCPs[0].x * padfGeoTransform[1];
        padfGeoTransform[3] = pasGCPs[0].ne.n - pasGCPs[0].y * padfGeoTransform[5];
        return TRUE;
    }

/* -------------------------------------------------------------------- */
/* In the general case, do a least squares error approximation by       */
/* solving the equation Sum[(A - B*x + C*y - Lon)^2] = minimum      */
/* -------------------------------------------------------------------- */

    double sum_x = 0.0, sum_y = 0.0, sum_xy = 0.0, sum_xx = 0.0, sum_yy = 0.0;
    double sum_Lon = 0.0, sum_Lonx = 0.0, sum_Lony = 0.0;
    double sum_Lat = 0.0, sum_Latx = 0.0, sum_Laty = 0.0;
    double divisor;

    for (i = 0; i < nGCPCount; ++i) {
        sum_x += pasGCPs[i].x;
        sum_y += pasGCPs[i].y;
        sum_xy += pasGCPs[i].x * pasGCPs[i].y;
        sum_xx += pasGCPs[i].x * pasGCPs[i].x;
        sum_yy += pasGCPs[i].y * pasGCPs[i].y;
        sum_Lon += pasGCPs[i].ne.e;
        sum_Lonx += pasGCPs[i].ne.e * pasGCPs[i].x;
        sum_Lony += pasGCPs[i].ne.e * pasGCPs[i].y;
        sum_Lat += pasGCPs[i].ne.n;
        sum_Latx += pasGCPs[i].ne.n * pasGCPs[i].x;
        sum_Laty += pasGCPs[i].ne.n * pasGCPs[i].y;
    }

    divisor = nGCPCount * (sum_xx * sum_yy - sum_xy * sum_xy)
        + 2 * sum_x * sum_y * sum_xy - sum_y * sum_y * sum_xx
        - sum_x * sum_x * sum_yy;

/* -------------------------------------------------------------------- */
/*      If the divisor is zero, there is no valid solution.             */
/* -------------------------------------------------------------------- */
    if (divisor == 0.0)
        return FALSE;

/* -------------------------------------------------------------------- */
/*      Compute top/left origin.                                        */
/* -------------------------------------------------------------------- */

    padfGeoTransform[0] = (sum_Lon * (sum_xx * sum_yy - sum_xy * sum_xy)
                           + sum_Lonx * (sum_y * sum_xy - sum_x *  sum_yy)
                           + sum_Lony * (sum_x * sum_xy - sum_y * sum_xx))
        / divisor;

    padfGeoTransform[3] = (sum_Lat * (sum_xx * sum_yy - sum_xy * sum_xy)
                           + sum_Latx * (sum_y * sum_xy - sum_x *  sum_yy)
                           + sum_Laty * (sum_x * sum_xy - sum_y * sum_xx))
        / divisor;

/* -------------------------------------------------------------------- */
/*      Compute X related coefficients.                                 */
/* -------------------------------------------------------------------- */
    padfGeoTransform[1] = (sum_Lon * (sum_y * sum_xy - sum_x * sum_yy)
                           + sum_Lonx * (nGCPCount * sum_yy - sum_y * sum_y)
                           + sum_Lony * (sum_x * sum_y - sum_xy * nGCPCount))
        / divisor;

    padfGeoTransform[2] = (sum_Lon * (sum_x * sum_xy - sum_y * sum_xx)
                           + sum_Lonx * (sum_x * sum_y - nGCPCount * sum_xy)
                           + sum_Lony * (nGCPCount * sum_xx - sum_x * sum_x))
        / divisor;

/* -------------------------------------------------------------------- */
/*      Compute Y related coefficients.                                 */
/* -------------------------------------------------------------------- */
    padfGeoTransform[4] = (sum_Lat * (sum_y * sum_xy - sum_x * sum_yy)
                           + sum_Latx * (nGCPCount * sum_yy - sum_y * sum_y)
                           + sum_Laty * (sum_x * sum_y - sum_xy * nGCPCount))
        / divisor;

    padfGeoTransform[5] = (sum_Lat * (sum_x * sum_xy - sum_y * sum_xx)
                           + sum_Latx * (sum_x * sum_y - nGCPCount * sum_xy)
                           + sum_Laty * (nGCPCount * sum_xx - sum_x * sum_x))
        / divisor;

/* -------------------------------------------------------------------- */
/*      Now check if any of the input points fit this poorly.           */
/* -------------------------------------------------------------------- */
    if( !bApproxOK )
    {
        double dfPixelSize = ABS(padfGeoTransform[1])
            + ABS(padfGeoTransform[2])
            + ABS(padfGeoTransform[4])
            + ABS(padfGeoTransform[5]);

        for( i = 0; i < nGCPCount; i++ )
        {
            double      dfErrorX, dfErrorY;

            dfErrorX =
                (pasGCPs[i].x * padfGeoTransform[1]
                 + pasGCPs[i].y * padfGeoTransform[2]
                 + padfGeoTransform[0])
                - pasGCPs[i].ne.e;
            dfErrorY =
                (pasGCPs[i].x * padfGeoTransform[4]
                 + pasGCPs[i].y * padfGeoTransform[5]
                 + padfGeoTransform[3])
                - pasGCPs[i].ne.n;

            if( ABS(dfErrorX) > 0.25 * dfPixelSize
                || ABS(dfErrorY) > 0.25 * dfPixelSize )
                return FALSE;
        }
    }

    return TRUE;
}

/**
 * Invert Geotransform.
 *
 * This function will invert a standard 3x2 set of GeoTransform coefficients.
 * This converts the equation from being pixel to geo to being geo to pixel.
 *
 * @param gt_in Input geotransform (six doubles - unaltered).
 * @param gt_out Output geotransform (six doubles - updated).
 *
 * @return TRUE on success or FALSE if the equation is uninvertable.
 */

int GDALInvGeoTransform( double *gt_in, double *gt_out )

{
    double  det, inv_det;

    /* we assume a 3rd row that is [1 0 0] */

    /* Compute determinate */

    det = gt_in[1] * gt_in[5] - gt_in[2] * gt_in[4];

    if( fabs(det) < 0.000000000000001 )
        return 0;

    inv_det = 1.0 / det;

    /* compute adjoint, and devide by determinate */

    gt_out[1] =  gt_in[5] * inv_det;
    gt_out[4] = -gt_in[4] * inv_det;

    gt_out[2] = -gt_in[2] * inv_det;
    gt_out[5] =  gt_in[1] * inv_det;

    gt_out[0] = ( gt_in[2] * gt_in[3] - gt_in[0] * gt_in[5]) * inv_det;
    gt_out[3] = (-gt_in[1] * gt_in[3] + gt_in[0] * gt_in[4]) * inv_det;

    return 1;
}

/**
 * Apply GeoTransform to x/y coordinate.
 *
 * Applies the following computation, converting a (pixel,line) coordinate
 * into a georeferenced (geo_x,geo_y) location.
 *
 *  *pdfGeoX = padfGeoTransform[0] + dfPixel * padfGeoTransform[1]
 *                                 + dfLine  * padfGeoTransform[2];
 *  *pdfGeoY = padfGeoTransform[3] + dfPixel * padfGeoTransform[4]
 *                                 + dfLine  * padfGeoTransform[5];
 *
 * @param padfGeoTransform Six coefficient GeoTransform to apply.
 * @param dfPixel Input pixel position.
 * @param dfLine Input line position.
 * @param *pdfGeoX output location where GeoX (easting/longitude) location is placed.
 * @param *pdfGeoY output location where GeoX (northing/latitude) location is placed.
 */

void GDALApplyGeoTransform( double *padfGeoTransform,
                            double dfPixel, double dfLine,
                            double *pdfGeoX, double *pdfGeoY )
{
    *pdfGeoX = padfGeoTransform[0] + dfPixel * padfGeoTransform[1] + dfLine  * padfGeoTransform[2];
    *pdfGeoY = padfGeoTransform[3] + dfPixel * padfGeoTransform[4] + dfLine  * padfGeoTransform[5];
}
